package fem2.element;

import fem2.Edge;
import fem2.MaterialModel;
import fem2.MeshPart;
import fem2.NotImplementedError;
import fem2.enu.MassMatrixType;
import fem2.enu.MeshPartType;
import fem2.material.StVenantKirchhoffMM;
import inf.jlinalg.Array2DMatrix;
import inf.jlinalg.ArrayVector;
import inf.jlinalg.BLAM;
import inf.jlinalg.IMatrix;
import inf.jlinalg.IMatrixReadOnly;
import inf.jlinalg.IVector;
import inf.jlinalg.IVectorReadOnly;

/**
 * @author hbui
 * 
 */
public class CrisfieldTrusssElement extends StructuralElement {

	private double preStress;

	private double E, A, L;
	private IVectorReadOnly Xe;

	private final static IMatrix A2 = new Array2DMatrix(new double[][] { //
			{ 1, 0, -1, 0 }, //
					{ 0, 1, 0, -1 }, //
					{ -1, 0, 1, 0 }, //
					{ 0, -1, 0, 1 } //
			});

	private final static IMatrix A3 = new Array2DMatrix(new double[][] { //
			{ 1, 0, 0, -1, 0, 0 }, //
					{ 0, 1, 0, 0, -1, 0 }, //
					{ 0, 0, 1, 0, 0, -1 }, //
					{ -1, 0, 0, 1, 0, 0 }, //
					{ 0, -1, 0, 0, 1, 0 }, //
					{ 0, 0, -1, 0, 0, 1 } //
			});

	/**
	 * @param mp
	 *            mesh part
	 * @param mm
	 *            material model
	 * @param preStress
	 *            pre-stress in the truss
	 */
	public CrisfieldTrusssElement(MeshPart mp, MaterialModel mm, double preStress) {
		super(mp, mm);
		if (!(mp instanceof Edge)) {
			throw new Error("Crisfield truss element must be defined on an edge");
		}
		if (mp.getType() != MeshPartType.LINE2) {
			throw new Error("Crisfield truss element must be defined on a 2-node line");
		}
		if (!(mm instanceof StVenantKirchhoffMM)) {
			throw new Error(
					"Crisfield truss element works with StVenantKirchhoff material model only");
		}
		this.preStress = preStress;
	}

	/**
	 * @return the mesh part as Edge
	 */
	public Edge getEdge() {
		return (Edge) getMeshPart();
	}

	@Override
	public void computeB(MeshPart mp, IMatrix b, double... xi) {
		throw new Error("this method is not available for this type of element");
	}

	private IMatrix computeA() {
		int dim = getMeshPart().getDimension();
		if (dim == 2) {
			return A2;
		} else if (dim == 3) {
			return A3;
		} else {
			throw new Error("invalid dimension");
		}
	}

	@Override
	public IMatrixReadOnly computeKte(IMatrix ke) {
		IVector ue = new ArrayVector(getMeshPart().getUHatLocal().clone());
		IVector tmp = (IVector) ue.clone();
		BLAM.add(1.0, Xe, ue);
		BLAM.multiply(1.0, BLAM.NO_TRANSPOSE, computeA(), ue, 0.0, tmp);
		BLAM.multiply(E * A / Math.pow(L, 3), tmp, tmp, ke);
		ke = (IMatrix) computeKge(ke);
		return ke;
	}

	@Override
	public IVectorReadOnly computeRie(IVector rie) {
		IVector ue = new ArrayVector(getMeshPart().getUHatLocal().clone());
		IVector tmp = (IVector) ue.clone();
		BLAM.add(1.0, Xe, ue);
		BLAM.multiply(1.0, BLAM.NO_TRANSPOSE, computeA(), ue, 0.0, tmp);
		double S11 = getStress(0).get(0);
		BLAM.add(A / L * (S11 + preStress), tmp, rie);
		return rie;
	}

	@Override
	public IVectorReadOnly computeRes(IVector res) {
		/*
		 * TODO
		 */
		throw new NotImplementedError();
	}

	@Override
	public IMatrixReadOnly computeMe(IMatrix me, MassMatrixType massType) {
		/*
		 * TODO
		 */
		throw new NotImplementedError();
	}

	@Override
	public IMatrixReadOnly computeKge(IMatrix ke) {
		double S11 = getStress(0).get(0);
		BLAM.add(A / L * (S11 + preStress), computeA(), ke);
		return ke;
	}

	@Override
	public IVector computeRegularStrain(double... xi) {
		IVector ue = new ArrayVector(getMeshPart().getUHatLocal().clone());
		IVector Aue = (IVector) ue.clone();
		BLAM.multiply(1.0, BLAM.NO_TRANSPOSE, computeA(), ue, 0.0, Aue);
		BLAM.scale(0.5, ue);
		BLAM.add(1.0, Xe, ue);// ue is (Xe+0.5*ue) now
		IVector E = new ArrayVector(1);
		E.set(0, BLAM.dot(ue, Aue) / Math.pow(L, 2));
		return E;
	}

	@Override
	public IVector computeStrain(double... xi) {
		return computeRegularStrain(xi);
	}

	// @Override
	// public double computeStrainEnergy() {
	// /*
	// * TODO
	// */
	// throw new NotImplementedError();
	// }

	@Override
	public void Initialize() {
		/*
		 * initialize the sigma array for this element
		 */
		int ng = getNumIntegrationPoints();
		for (int i = 0; i < ng; i++) {
			sigma.add(new ArrayVector(1));
		}
		/*
		 * initialize material
		 */
		getMaterialModel().Initialize(this);

		L = getEdge().getLength();
		E = getMaterialModel().getConstitutiveMatrix(this, 0).get(0, 0);
		A = getMaterialModel().getSectionProperty();
		Xe = new ArrayVector(getMeshPart().getPosition());
	}
}
